#include "GameTools.hpp"
#define LIMIT 150
#define AXIS_MAX 65535
uint16_t NumberSub(uint16_t a, uint16_t b)
{
    return (a >= b) ? (a - b) : (b - a);
}

void Axis_X4(reprot_type *reprot)
{
    reprot->Axis_Rx = reprot->Axis_Rx << 4;
    reprot->Axis_Ry = reprot->Axis_Ry << 4;
    reprot->Throttle = reprot->Throttle << 4;
    reprot->Brake = reprot->Brake << 4;
}

void Amplitude_Filter(reprot_type *reprot, uint16_t wava_filter)
{
    wava_filter *= 10;
    if (NumberSub(reprot->Axis_Rx, reprot->Last_Axis_Rx) > wava_filter)
    {
        reprot->Last_Axis_Rx = reprot->Axis_Rx;
    }
    else
    {
        reprot->Axis_Rx = reprot->Last_Axis_Rx;
    }

    if (NumberSub(reprot->Axis_Ry, reprot->Last_Axis_Ry) > wava_filter)
    {
        reprot->Last_Axis_Ry = reprot->Axis_Ry;
    }
    else
    {
        reprot->Axis_Ry = reprot->Last_Axis_Ry;
    }

    if (NumberSub(reprot->Throttle, reprot->Last_Throttle) > wava_filter)
    {
        reprot->Last_Throttle = reprot->Throttle;
    }
    else
    {
        reprot->Throttle = reprot->Last_Throttle;
    }

    if (NumberSub(reprot->Brake, reprot->Last_Brake) > wava_filter)
    {
        reprot->Last_Brake = reprot->Brake;
    }
    else
    {
        reprot->Brake = reprot->Last_Brake;
    }
}

void Filter(reprot_type *reprot, uint16_t wava_filter)
{
    float K = 1 - wava_filter / 100.0f;
    reprot->Last_Axis_Rx += K * (reprot->Axis_Rx - reprot->Last_Axis_Rx);
    reprot->Axis_Rx = reprot->Last_Axis_Rx;

    reprot->Last_Axis_Ry += K * (reprot->Axis_Ry - reprot->Last_Axis_Ry);
    reprot->Axis_Ry = reprot->Last_Axis_Ry;

    reprot->Last_Brake += K * (reprot->Brake - reprot->Last_Brake);
    reprot->Brake = reprot->Last_Brake;

    reprot->Last_Throttle += K * (reprot->Throttle - reprot->Last_Throttle);
    reprot->Throttle = reprot->Last_Throttle;
}

void Low_Pass_Filter(reprot_type *reprot, uint16_t wava_filter)
{
    float temp = wava_filter / 100.0f;
    reprot->Axis_Rx = temp * reprot->Last_Axis_Rx + (1 - temp) * reprot->Axis_Rx;
    reprot->Last_Axis_Rx = reprot->Axis_Rx;

    reprot->Axis_Ry = temp * reprot->Last_Axis_Ry + (1 - temp) * reprot->Axis_Ry;
    reprot->Last_Axis_Ry = reprot->Axis_Ry;

    reprot->Brake = temp * reprot->Last_Brake + (1 - temp) * reprot->Brake;
    reprot->Last_Brake = reprot->Brake;

    reprot->Throttle = temp * reprot->Last_Throttle + (1 - temp) * reprot->Throttle;
    reprot->Last_Throttle = reprot->Throttle;
}

float KalmanFilter(Kalman_Filter_Par *kfp,float input){
    kfp->NowP = kfp->LastP + kfp->Q;
    kfp->Kg = kfp->NowP / (kfp->NowP + kfp->R);
    kfp->out = kfp->out + kfp->Kg * (input - kfp->out);
    kfp->LastP = (1 - kfp->Kg) * kfp->NowP;
    return kfp->out;
}

void Axis_Config(reprot_type *reprot, Dead_Axis *Dead)
{
    if (Dead->Axis_Rx_ENABLE != 0)
    {
        if (reprot->Axis_Rx <= Dead->Axis_Rx_Low)
        {
            reprot->Axis_Rx = 0;
        }
        else
        {
            reprot->Axis_Rx -= Dead->Axis_Rx_Low;
        }
        if (reprot->Axis_Rx >= AXIS_MAX / Dead->Axis_Rx_Zoom)
        {
            reprot->Axis_Rx = 65535;
        }
        else
        {
            reprot->Axis_Rx *= Dead->Axis_Rx_Zoom;
        }
    }
    else
    {
        reprot->Axis_Rx = 0;
    }
    if (Dead->Axis_Ry_ENABLE != 0)
    {
        if (reprot->Axis_Ry <= Dead->Axis_Ry_Low)
        {
            reprot->Axis_Ry = 0;
        }
        else
        {
            reprot->Axis_Ry -= Dead->Axis_Ry_Low;
        }
        if (reprot->Axis_Ry >= AXIS_MAX / Dead->Axis_Ry_Zoom)
        {
            reprot->Axis_Ry = AXIS_MAX;
        }
        else
        {
            reprot->Axis_Ry *= Dead->Axis_Ry_Zoom;
        }
    }
    else
    {
        reprot->Axis_Ry = 0;
    }
    if (Dead->Axis_Brake_ENABLE != 0)
    {
        if (reprot->Brake <= Dead->Brake_Low)
        {
            reprot->Brake = 0;
        }
        else
        {
            reprot->Brake -= Dead->Brake_Low;
        }
        if (reprot->Brake >= AXIS_MAX / Dead->Brake_Zoom)
        {
            reprot->Brake = AXIS_MAX;
        }
        else
        {
            reprot->Brake *= Dead->Brake_Zoom;
        }
    }
    else
    {
        reprot->Brake = 0;
    }
    if (Dead->Axis_Throttle_ENABLE != 0)
    {
        if (reprot->Throttle <= Dead->Throttle_Low)
        {
            reprot->Throttle = 0;
        }
        else
        {
            reprot->Throttle -= Dead->Throttle_Low;
        }
        if (reprot->Throttle >= AXIS_MAX / Dead->Throttle_Zoom)
        {
            reprot->Throttle = AXIS_MAX;
        }
        else
        {
            reprot->Throttle *= Dead->Throttle_Zoom;
        }
    }
    else
    {
        reprot->Throttle = 0;
    }
}

void Axis_Init_Config(Dead_Axis *dead, out_reprot *config)
{
    dead->Axis_Brake_ENABLE = config->ENABLE_Brake;
    dead->Axis_Throttle_ENABLE = config->ENABLE_Throttle;
    dead->Axis_Rx_ENABLE = config->ENABLE_Rx;
    dead->Axis_Ry_ENABLE = config->ENABLE_Ry;
    dead->Brake_Low = AXIS_MAX * config->dead_Brake_Low / 100.0f;
    dead->Throttle_Low = AXIS_MAX * config->dead_Throttle_Low / 100.0f;
    dead->Axis_Rx_Low = AXIS_MAX * config->dead_Rx_Low / 100.0f;
    dead->Axis_Ry_Low = AXIS_MAX * config->dead_Ry_Low / 100.0f;
    dead->Brake_Zoom = AXIS_MAX / ((AXIS_MAX * config->dead_Brake_High / 100.0f) - dead->Brake_Low);
    // printf("%f %f %d \n",dead->Brake_Zoom,AXIS_MAX * config->dead_Brake_High / 100.0f,dead->Axis_Rx_ENABLE);
    dead->Throttle_Zoom = AXIS_MAX / ((AXIS_MAX * config->dead_Throttle_High / 100.0f) - dead->Throttle_Low);
    dead->Axis_Rx_Zoom = AXIS_MAX / ((AXIS_MAX * config->dead_Rx_High / 100.0f) - dead->Axis_Rx_Low);
    dead->Axis_Ry_Zoom = AXIS_MAX / ((AXIS_MAX * config->dead_Ry_High / 100.0f) - dead->Axis_Ry_Low);
}